/**
 * @file adaption_imu.c
 * @author your name (you@domain.com)
 * @brief 此文件为上层提供统一的API接口，方便模块的更换，如有模块更换，只需要
 *        替换此文件的实现过程，不需要改头文件和上层代码。
 * @version 0.1
 * @date 2022-12-20
 * 
 * @copyright Copyright (c) 2022
 * 
 */
#include "adaption_imu.h"
#include "system_support.h"
#include "Icm20602.h"
#include "icm20602_conf.h"
#include "DeviceCharacteristic.h"

#define IMU_MODULE_TAG    "imu_module"

#define ACCEL_SENSITIVITY   8192.f
#define GRAVITY     9.8f
#define GYRO_SENSITIVITY   131.f
#define DEGREE_TO_RAD   3.14159f/360


/* icm20602设备实例 */
inv_icm20602_t icm_device;

static void   IMU_Calibration(IMU_Typedef *IMU_Handle);
static int8_t imu_direction_init(IMU_Typedef *IMU_Handle);
static void imu_direction_convert(IMU_Typedef *IMU_Handle);
static void imu_set_offset(IMU_Typedef *IMU_Handle, float _accel_offset[3], float _gyro_offset[3], float _acc_ratio[3]);
static void imu_rectify(IMU_Typedef *IMU_Handle);

int8_t IMU_init(IMU_Typedef *IMU_Handle)
{
    int8_t ret;
    uint8_t whoami = 0;

    ret = inv_icm20602_get_whoami(&icm_device, &whoami);
	if(ret < 0){
        elog_error(IMU_MODULE_TAG, "ICM20602 get WHOAMI fail");
        return ret;
	}
	if(whoami != 0x12){
		elog_error(IMU_MODULE_TAG, "ICM20602 WHOAMI=0x%02x", whoami);
		return -1;
	}

    ret = inv_icm20602_initialize(&icm_device);
    if(ret < 0){
        elog_error(IMU_MODULE_TAG, "ICM20602 init fail");
        return ret;
    }
	
	ret += inv_icm20602_enable_sensor(&icm_device, INV_ICM20602_SENSOR_ACCEL, 1);
	ret += inv_icm20602_enable_sensor(&icm_device, INV_ICM20602_SENSOR_GYRO, 1);
	ret += inv_icm20602_enable_sensor(&icm_device, INV_ICM20602_SENSOR_TEMPERATURE, 1);
	if(ret < 0){
        elog_error(IMU_MODULE_TAG, "ICM20602 enable sensor fail");
        return ret;
	}

    ret += inv_icm20602_set_sensor_period(&icm_device, INV_ICM20602_SENSOR_ACCEL, OUTPUT_DATA_RATE_MS);    
    ret += inv_icm20602_set_sensor_period(&icm_device, INV_ICM20602_SENSOR_GYRO, OUTPUT_DATA_RATE_MS);    
    if(ret < 0){
        elog_error(IMU_MODULE_TAG, "ICM20602 set sensor period fail");
        return ret;
    }

    ret += inv_icm20602_set_accel_fullscale(&icm_device, ACCEL_FULL_SCALE);
    ret += inv_icm20602_set_gyro_fullscale(&icm_device, GYRO_FULL_SCALE);
    if(ret < 0){
        elog_error(IMU_MODULE_TAG, "ICM20602 set sensor full scale fail");
        return ret;
    }
    
    imu_set_offset(IMU_Handle, AccBias, GyroBias, AccRatio);

    /* 设置传感器方向 */
    ret += imu_direction_init(IMU_Handle);
	
    if(ret == 0)
	    elog_info(IMU_MODULE_TAG, "ICM20602 init success");
	
    return ret;
}

void IMU_GetAccRaw(IMU_Typedef *IMU_Handle, int16_t *_acc_raw)
{
    for(uint8_t i = 0; i < 3; i ++)
    {
        _acc_raw[i] = IMU_Handle->acc_raw[i];
    }
}

void IMU_GetGyroRaw(IMU_Typedef *IMU_Handle, int16_t *_gyro_raw)
{
    for(uint8_t i = 0; i < 3; i ++)
    {
        _gyro_raw[i] = IMU_Handle->gyro_raw[i];
    }
}

void IMU_GetTempRaw(IMU_Typedef *IMU_Handle, int16_t *_temp_raw)
{
    *_temp_raw = IMU_Handle->temp_raw;
}


void IMU_GetAccRel(IMU_Typedef *IMU_Handle, float *_acc_rel)
{
    for(uint8_t i = 0; i < 3; i ++)
    {
        _acc_rel[i] = IMU_Handle->acc_rel[i];
    }
}

void IMU_GetGyroRel(IMU_Typedef *IMU_Handle, float *_gyro_rel)
{
    for(uint8_t i = 0; i < 3; i ++)
    {
        _gyro_rel[i] = IMU_Handle->gyro_rel[i];
    }
}

/**
 * @brief IMU数据更新接口
 * 
 * @param IMU_Handle 
 * @return int8_t 
 */
int8_t IMU_UpdateData(IMU_Typedef *IMU_Handle)
{
    int8_t ret = 0;
    /* 获取传感器数据 */
    ret = inv_icm20602_poll_sensor_data_reg(&icm_device, \
    IMU_Handle->acc_raw, &IMU_Handle->temp_raw, IMU_Handle->gyro_raw);
    if(ret < 0){
        elog_error(IMU_MODULE_TAG, "polling data error");
        return -1;
    }

    /* 记录时间戳 */
    IMU_Handle->last_record_time_us = IMU_Handle->record_time_us;
    IMU_Handle->record_time_us = Get_SystemTimer_us();
    if(IMU_Handle->last_record_time_us == 0){
        IMU_Handle->derta_record_time_us = 0;
        IMU_Handle->update_freq_Hz = 0;
    }
    else{
        IMU_Handle->derta_record_time_us = IMU_Handle->record_time_us - IMU_Handle->last_record_time_us;
        IMU_Handle->update_freq_Hz = 1000000 / IMU_Handle->derta_record_time_us;
    }

    /* 方向转换 */
    imu_direction_convert(IMU_Handle);

    /* 数值矫正 */
    imu_rectify(IMU_Handle);

    /* 采集数据计算累计均值 */
    IMU_Calibration(IMU_Handle);

    return ret;
}

/**
 * @brief IMU数据校准程序,将数据采集处理后把偏置打印出来
 * 
 */
static void IMU_Calibration(IMU_Typedef *IMU_Handle)
{
#if  SELF_CALLIBRATION_ENABLE == 1
    if(IMU_Handle->record_cnt < IMU_CALIBRATION_TIME){
        IMU_Handle->gyro_sum[0] += IMU_Handle->gyro_rel[0];
        IMU_Handle->gyro_sum[1] += IMU_Handle->gyro_rel[1];
        IMU_Handle->gyro_sum[2] += IMU_Handle->gyro_rel[2];
        IMU_Handle->record_cnt ++;
    }
    else if(IMU_Handle->record_cnt == IMU_CALIBRATION_TIME){
        /* 计算采样值的均值作为校准值 */
        IMU_Handle->gyro_offset[0] = IMU_Handle->gyro_sum[0] / IMU_CALIBRATION_TIME;
        IMU_Handle->gyro_offset[1] = IMU_Handle->gyro_sum[1] / IMU_CALIBRATION_TIME;
        IMU_Handle->gyro_offset[2] = IMU_Handle->gyro_sum[2] / IMU_CALIBRATION_TIME;

        /* 打印出三轴校准值 */
        elog_info(IMU_MODULE_TAG, "self calibration gyro_offset: X:%f,Y:%f,Z:%f", \
        IMU_Handle->gyro_offset[0], IMU_Handle->gyro_offset[1], IMU_Handle->gyro_offset[2]);
		IMU_Handle->record_cnt ++;
    }
#endif
}

/**
 * @brief IMU方向转换
 * 
 * @param IMU_Handle 
 */
static void imu_direction_convert(IMU_Typedef *IMU_Handle)
{
	// 方向变换
	IMU_Handle->acc_rot[0] = IMU_Handle->direction[0]*(IMU_Handle->acc_raw[IMU_Handle->direction[1]]);
	IMU_Handle->acc_rot[1] = IMU_Handle->direction[2]*(IMU_Handle->acc_raw[IMU_Handle->direction[3]]);
	IMU_Handle->acc_rot[2] = IMU_Handle->direction[4]*(IMU_Handle->acc_raw[IMU_Handle->direction[5]]);
    
	IMU_Handle->gyro_rot[0] = IMU_Handle->direction[0]*(IMU_Handle->gyro_raw[IMU_Handle->direction[1]]);
	IMU_Handle->gyro_rot[1] = IMU_Handle->direction[2]*(IMU_Handle->gyro_raw[IMU_Handle->direction[3]]);
	IMU_Handle->gyro_rot[2] = IMU_Handle->direction[4]*(IMU_Handle->gyro_raw[IMU_Handle->direction[5]]);
}

/**
 * @brief IMU数据矫正
 * 
 * @param IMU_Handle 
 */
static void imu_rectify(IMU_Typedef *IMU_Handle)
{
    /* 加速度计测量值矫正 */
	IMU_Handle->acc_rel[0] = IMU_Handle->acc_ratio[0]*(IMU_Handle->acc_rot[0]/ACCEL_SENSITIVITY*GRAVITY + IMU_Handle->acc_offset[0]);
	IMU_Handle->acc_rel[1] = IMU_Handle->acc_ratio[1]*(IMU_Handle->acc_rot[1]/ACCEL_SENSITIVITY*GRAVITY + IMU_Handle->acc_offset[1]);
	IMU_Handle->acc_rel[2] = IMU_Handle->acc_ratio[2]*(IMU_Handle->acc_rot[2]/ACCEL_SENSITIVITY*GRAVITY + IMU_Handle->acc_offset[2]);
    memcpy(IMU_Handle->imu_msg.acc,IMU_Handle->acc_rel,sizeof(IMU_Handle->acc_rel));
    
    /* 陀螺仪测量值矫正 */
	IMU_Handle->gyro_rel[0] = IMU_Handle->gyro_rot[0]/GYRO_SENSITIVITY*DEGREE_TO_RAD - IMU_Handle->gyro_offset[0];
	IMU_Handle->gyro_rel[1] = IMU_Handle->gyro_rot[1]/GYRO_SENSITIVITY*DEGREE_TO_RAD - IMU_Handle->gyro_offset[1];
	IMU_Handle->gyro_rel[2] = IMU_Handle->gyro_rot[2]/GYRO_SENSITIVITY*DEGREE_TO_RAD - IMU_Handle->gyro_offset[2];
    memcpy(IMU_Handle->imu_msg.gyro,IMU_Handle->gyro_rel,sizeof(IMU_Handle->gyro_rel));
}


/**
 * @brief 字符串转矩阵，将"+X+Y+Z"转成 +1 0 +1 1 +1 2
 * 
 * @param IMU_Handle IMU句柄
 * @param direction_string 字符串
 * @return int8_t 
 */
static int8_t imu_direction_init(IMU_Typedef *IMU_Handle)
{
    char *direction_string = IMU_DIRECTION;
    
	for(int i=0;i<6;)
	{
		switch(direction_string[i])
		{
			case '+':IMU_Handle->direction[i] = 1;break;
			case '-':IMU_Handle->direction[i] =-1;break;
			default:return -1;
		}
		i++;
		switch(direction_string[i])
		{
			case 'X':IMU_Handle->direction[i] = 0;break;
			case 'Y':IMU_Handle->direction[i] = 1;break;
			case 'Z':IMU_Handle->direction[i] = 2;break;
			default:return -1;
		}
		i++;
	}
	return 0;
}

static void imu_set_offset(IMU_Typedef *IMU_Handle, float _accel_offset[3], float _gyro_offset[3], float _acc_ratio[3])
{
    IMU_Handle->acc_offset[0] = _accel_offset[0];
    IMU_Handle->acc_offset[1] = _accel_offset[1];
    IMU_Handle->acc_offset[2] = _accel_offset[2];

    IMU_Handle->gyro_offset[0] = _gyro_offset[0];
    IMU_Handle->gyro_offset[1] = _gyro_offset[1];
    IMU_Handle->gyro_offset[2] = _gyro_offset[2];
    
    IMU_Handle->acc_ratio[0] = _acc_ratio[0];
    IMU_Handle->acc_ratio[1] = _acc_ratio[1];
    IMU_Handle->acc_ratio[2] = _acc_ratio[2];
}
